﻿/*
 * File: force_dis_8new.c
 *
 * MATLAB Coder version            : 5.0
 * C/C++ source code generated on  : 30-Mar-2021 17:14:58
 */

 /* Include Files */
#include "force_dis_8.h"
#include "rt_nonfinite.h"
#include <float.h>
#include <math.h>

/* Variable Definitions */
static boolean_T isInitialized_force_dis_8new = false;

/* Function Declarations */
static void b_cosd(double *x);
static void b_inv(const double x[16], double y[16]);
static void b_sind(double *x);
static void inv(const double x[4], double y[4]);
static double rt_remd_snf(double u0, double u1);

/* Function Definitions */

/*
 * Arguments    : double *x
 * Return Type  : void
 */
static void b_cosd(double *x)
{
	double absx;
	signed char n;
	if (rtIsInf(*x) || rtIsNaN(*x)) {
		*x = rtNaN;
	}
	else {
		*x = rt_remd_snf(*x, 360.0);
		absx = fabs(*x);
		if (absx > 180.0) {
			if (*x > 0.0) {
				*x -= 360.0;
			}
			else {
				*x += 360.0;
			}

			absx = fabs(*x);
		}

		if (absx <= 45.0) {
			*x *= 0.017453292519943295;
			n = 0;
		}
		else if (absx <= 135.0) {
			if (*x > 0.0) {
				*x = 0.017453292519943295 * (*x - 90.0);
				n = 1;
			}
			else {
				*x = 0.017453292519943295 * (*x + 90.0);
				n = -1;
			}
		}
		else if (*x > 0.0) {
			*x = 0.017453292519943295 * (*x - 180.0);
			n = 2;
		}
		else {
			*x = 0.017453292519943295 * (*x + 180.0);
			n = -2;
		}

		if (n == 0) {
			*x = cos(*x);
		}
		else if (n == 1) {
			*x = -sin(*x);
		}
		else if (n == -1) {
			*x = sin(*x);
		}
		else {
			*x = -cos(*x);
		}
	}
}

/*
 * Arguments    : const double x[16]
 *                double y[16]
 * Return Type  : void
 */
static void b_inv(const double x[16], double y[16])
{
	int i;
	signed char ipiv[4];
	double b_x[16];
	int j;
	signed char p[4];
	int mmj_tmp;
	int b;
	int jj;
	int jp1j;
	int jy;
	int iy;
	int ix;
	double smax;
	int k;
	signed char i1;
	double s;
	for (i = 0; i < 16; i++) {
		y[i] = 0.0;
		b_x[i] = x[i];
	}

	ipiv[0] = 1;
	ipiv[1] = 2;
	ipiv[2] = 3;
	for (j = 0; j < 3; j++) {
		mmj_tmp = 2 - j;
		b = j * 5;
		jj = j * 5;
		jp1j = b + 2;
		jy = 4 - j;
		iy = 0;
		ix = b;
		smax = fabs(b_x[jj]);
		for (k = 2; k <= jy; k++) {
			ix++;
			s = fabs(b_x[ix]);
			if (s > smax) {
				iy = k - 1;
				smax = s;
			}
		}

		if (b_x[jj + iy] != 0.0) {
			if (iy != 0) {
				iy += j;
				ipiv[j] = (signed char)(iy + 1);
				smax = b_x[j];
				b_x[j] = b_x[iy];
				b_x[iy] = smax;
				ix = j + 4;
				iy += 4;
				smax = b_x[ix];
				b_x[ix] = b_x[iy];
				b_x[iy] = smax;
				ix += 4;
				iy += 4;
				smax = b_x[ix];
				b_x[ix] = b_x[iy];
				b_x[iy] = smax;
				ix += 4;
				iy += 4;
				smax = b_x[ix];
				b_x[ix] = b_x[iy];
				b_x[iy] = smax;
			}

			i = (jj - j) + 4;
			for (k = jp1j; k <= i; k++) {
				b_x[k - 1] /= b_x[jj];
			}
		}

		jy = b + 4;
		iy = jj;
		for (k = 0; k <= mmj_tmp; k++) {
			smax = b_x[jy];
			if (b_x[jy] != 0.0) {
				ix = jj + 1;
				i = iy + 6;
				jp1j = (iy - j) + 8;
				for (b = i; b <= jp1j; b++) {
					b_x[b - 1] += b_x[ix] * -smax;
					ix++;
				}
			}

			jy += 4;
			iy += 4;
		}
	}

	p[0] = 1;
	p[1] = 2;
	p[2] = 3;
	p[3] = 4;
	if (ipiv[0] > 1) {
		jy = ipiv[0] - 1;
		iy = p[jy];
		p[jy] = 1;
		p[0] = (signed char)iy;
	}

	if (ipiv[1] > 2) {
		jy = ipiv[1] - 1;
		iy = p[jy];
		p[jy] = p[1];
		p[1] = (signed char)iy;
	}

	if (ipiv[2] > 3) {
		jy = ipiv[2] - 1;
		iy = p[jy];
		p[jy] = p[2];
		p[2] = (signed char)iy;
	}

	i1 = p[0];
	y[(p[0] - 1) << 2] = 1.0;
	for (j = 1; j < 5; j++) {
		if (y[(j + ((i1 - 1) << 2)) - 1] != 0.0) {
			i = j + 1;
			for (k = i; k < 5; k++) {
				iy = (k + ((i1 - 1) << 2)) - 1;
				y[iy] -= y[(j + ((i1 - 1) << 2)) - 1] * b_x[(k + ((j - 1) << 2)) - 1];
			}
		}
	}

	i1 = p[1];
	y[((p[1] - 1) << 2) + 1] = 1.0;
	for (j = 2; j < 5; j++) {
		if (y[(j + ((i1 - 1) << 2)) - 1] != 0.0) {
			i = j + 1;
			for (k = i; k < 5; k++) {
				iy = (k + ((i1 - 1) << 2)) - 1;
				y[iy] -= y[(j + ((i1 - 1) << 2)) - 1] * b_x[(k + ((j - 1) << 2)) - 1];
			}
		}
	}

	i1 = p[2];
	y[((p[2] - 1) << 2) + 2] = 1.0;
	for (j = 3; j < 5; j++) {
		jp1j = (j + ((i1 - 1) << 2)) - 1;
		if (y[jp1j] != 0.0) {
			i = j + 1;
			for (k = i; k < 5; k++) {
				iy = ((i1 - 1) << 2) + 3;
				y[iy] -= y[jp1j] * b_x[((j - 1) << 2) + 3];
			}
		}
	}

	y[((p[3] - 1) << 2) + 3] = 1.0;
	for (j = 0; j < 4; j++) {
		jy = j << 2;
		smax = y[jy + 3];
		if (smax != 0.0) {
			y[jy + 3] = smax / b_x[15];
			for (k = 0; k < 3; k++) {
				iy = k + jy;
				y[iy] -= y[jy + 3] * b_x[k + 12];
			}
		}

		smax = y[jy + 2];
		if (smax != 0.0) {
			y[jy + 2] = smax / b_x[10];
			for (k = 0; k < 2; k++) {
				iy = k + jy;
				y[iy] -= y[jy + 2] * b_x[k + 8];
			}
		}

		smax = y[jy + 1];
		if (smax != 0.0) {
			y[jy + 1] = smax / b_x[5];
			for (k = 0; k < 1; k++) {
				y[jy] -= y[jy + 1] * b_x[4];
			}
		}

		if (y[jy] != 0.0) {
			y[jy] /= b_x[0];
		}
	}
}

/*
 * Arguments    : double *x
 * Return Type  : void
 */
static void b_sind(double *x)
{
	double absx;
	signed char n;
	if (rtIsInf(*x) || rtIsNaN(*x)) {
		*x = rtNaN;
	}
	else {
		*x = rt_remd_snf(*x, 360.0);
		absx = fabs(*x);
		if (absx > 180.0) {
			if (*x > 0.0) {
				*x -= 360.0;
			}
			else {
				*x += 360.0;
			}

			absx = fabs(*x);
		}

		if (absx <= 45.0) {
			*x *= 0.017453292519943295;
			n = 0;
		}
		else if (absx <= 135.0) {
			if (*x > 0.0) {
				*x = 0.017453292519943295 * (*x - 90.0);
				n = 1;
			}
			else {
				*x = 0.017453292519943295 * (*x + 90.0);
				n = -1;
			}
		}
		else if (*x > 0.0) {
			*x = 0.017453292519943295 * (*x - 180.0);
			n = 2;
		}
		else {
			*x = 0.017453292519943295 * (*x + 180.0);
			n = -2;
		}

		if (n == 0) {
			*x = sin(*x);
		}
		else if (n == 1) {
			*x = cos(*x);
		}
		else if (n == -1) {
			*x = -cos(*x);
		}
		else {
			*x = -sin(*x);
		}
	}
}

/*
 * Arguments    : const double x[4]
 *                double y[4]
 * Return Type  : void
 */
static void inv(const double x[4], double y[4])
{
	double r;
	double t;
	if (fabs(x[1]) > fabs(x[0])) {
		r = x[0] / x[1];
		t = 1.0 / (r * x[3] - x[2]);
		y[0] = x[3] / x[1] * t;
		y[1] = -t;
		y[2] = -x[2] / x[1] * t;
		y[3] = r * t;
	}
	else {
		r = x[1] / x[0];
		t = 1.0 / (x[3] - r * x[2]);
		y[0] = x[3] / x[0] * t;
		y[1] = -r * t;
		y[2] = -x[2] / x[0] * t;
		y[3] = t;
	}
}

/*
 * Arguments    : double u0
 *                double u1
 * Return Type  : double
 */
static double rt_remd_snf(double u0, double u1)
{
	double y;
	double q;
	if (rtIsNaN(u0) || rtIsNaN(u1) || rtIsInf(u0)) {
		y = rtNaN;
	}
	else if (rtIsInf(u1)) {
		y = u0;
	}
	else if ((u1 != 0.0) && (u1 != trunc(u1))) {
		q = fabs(u0 / u1);
		if (!(fabs(q - floor(q + 0.5)) > DBL_EPSILON * q)) {
			y = 0.0 * u0;
		}
		else {
			y = fmod(u0, u1);
		}
	}
	else {
		y = fmod(u0, u1);
	}

	return y;
}

/*
 * 8自由度力分配
 * MODE 0俯仰反向修正   1横滚反向修正    2负数修正    3尺度修正
 * Arguments    : const double FALL[3]
 *                const double TALL[3]
 *                const double Fp[2]
 *                const double PC[3]
 *                double P1[3]
 *                double P2[3]
 *                double P3[3]
 *                double P4[3]
 *                const char G[4]
 *                const double ATT_NOW[3]
 *                double Div_Roll
 *                const char MODE[7]
 *                const char CAL_MODE[4]
 *                double F1c[3]
 *                double F2c[3]
 *                double F3c[3]
 *                double F4c[3]
 * Return Type  : void
 */
void force_dis_8new(const double FALL[3], const double TALL[3], const double Fp
	[2], const double PC[3], double P1[3], double P2[3], double
	P3[3], double P4[3], const char G[4], const double ATT_NOW[3],
	double Div_Roll, const char MODE[7], const char CAL_MODE[4],
	double F1c[3], double F2c[3], double F3c[3], double F4c[3])
{
	int G_NUM_tmp;
	int b_G_NUM_tmp;
	int c_G_NUM_tmp;
	int d_G_NUM_tmp;
	int G_NUM;
	double P1_reg_idx_0;
	double P2_reg_idx_0;
	double P3_reg_idx_0;
	double P4_reg_idx_0;
	double P1_reg_idx_1;
	double P2_reg_idx_1;
	double P3_reg_idx_1;
	double P4_reg_idx_1;
	double P1_reg_idx_2;
	double P2_reg_idx_2;
	double P3_reg_idx_2;
	double P4_reg_idx_2;
	double A[6];
	int i;
	int y_tmp_tmp;
	double y_tmp[6];
	double b_y_tmp[4];
	double dv[4];
	double PvL_idx_0;
	double F[4];
	double PvR_idx_0;
	double B[3];
	double b_B[5];
	double FF_idx_2;
	double b_F[2];
	double FH_idx_2;
	int i1;
	double B_idx_0;
	double b_A[20];
	double B_idx_1;
	double B_idx_2;
	double c_y_tmp[20];
	double PvR_idx_2;
	double d_y_tmp[16];
	double c_A[16];
	double e_y_tmp[16];
	if (!isInitialized_force_dis_8new) {
		//	force_dis_8new_initialize();
	}

	G_NUM_tmp = (unsigned char)G[0];
	b_G_NUM_tmp = (unsigned char)G[1];
	c_G_NUM_tmp = (unsigned char)G[2];
	d_G_NUM_tmp = (unsigned char)G[3];
	G_NUM = ((G_NUM_tmp + b_G_NUM_tmp) + c_G_NUM_tmp) + d_G_NUM_tmp;
	P1_reg_idx_0 = P1[0];
	P2_reg_idx_0 = P2[0];
	P3_reg_idx_0 = P3[0];
	P4_reg_idx_0 = P4[0];
	F1c[0] = 0.0;
	F2c[0] = 0.0;
	F3c[0] = 0.0;
	F4c[0] = 0.0;
	P1_reg_idx_1 = P1[1];
	P2_reg_idx_1 = P2[1];
	P3_reg_idx_1 = P3[1];
	P4_reg_idx_1 = P4[1];
	F1c[1] = 0.0;
	F2c[1] = 0.0;
	F3c[1] = 0.0;
	F4c[1] = 0.0;
	P1_reg_idx_2 = P1[2];
	P2_reg_idx_2 = P2[2];
	P3_reg_idx_2 = P3[2];
	P4_reg_idx_2 = P4[2];
	F1c[2] = 0.0;
	F2c[2] = 0.0;
	F3c[2] = 0.0;
	F4c[2] = 0.0;

	/* cmd test */
	/* ------------------------------------4 Leg--------------------------------- */
	/*      逆+（从箭头往里看） */
	/*       z   y 逆+ */
	/*       |  / */
	/*       | / */
	/*       |/ */
	/*       o—————x 逆+ */
	/* 1FL   2FR */
	/* 4BL   3BR */
	if ((G_NUM == 4) && ((unsigned char)CAL_MODE[3] == 0)) {
		/* --------------------------------四腿着地  垂直各为机头计算一次 */
		/* ----俯仰解算 */
		if (MODE[4] != '\x00') {
			/* 调节俯仰时多余腿的位置 */
			if ((G[0] != '\x00') && (G[2] != '\x00')) {
				P2[0] = P3[0];
				P2[1] = P1[1];
				P2[2] = P1[2];
				P4[0] = P1[0];
				P4[1] = P3[1];
				P4[2] = P3[2];
			}
			else {
				P1[0] = P4[0];
				P1[1] = P2[1];
				P1[2] = P2[2];
				P3[0] = P2[0];
				P3[1] = P4[1];
				P3[2] = P4[2];
			}
		}

		/* 计算虚拟腿 */
		/* MX   Ffz */
		/* MY   Fhz */
		A[0] = 1.0;
		A[3] = 1.0;
		A[1] = -(PC[1] - (P1[1] * 0.5 + P2[1] * 0.5));
		A[4] = -(PC[1] - (P3[1] * 0.5 + P4[1] * 0.5));
		A[2] = PC[0] - (P1[0] * 0.5 + P2[0] * 0.5);
		A[5] = PC[0] - (P3[0] * 0.5 + P4[0] * 0.5);
		for (i = 0; i < 3; i++) {
			y_tmp_tmp = i << 1;
			y_tmp[y_tmp_tmp] = A[i];
			y_tmp[y_tmp_tmp + 1] = A[i + 3];
		}

		for (i = 0; i < 2; i++) {
			PvL_idx_0 = y_tmp[i + 2];
			PvR_idx_0 = y_tmp[i + 4];
			for (y_tmp_tmp = 0; y_tmp_tmp < 2; y_tmp_tmp++) {
				b_y_tmp[i + (y_tmp_tmp << 1)] = (y_tmp[i] * A[3 * y_tmp_tmp] + PvL_idx_0
					* A[3 * y_tmp_tmp + 1]) + PvR_idx_0 * A[3 * y_tmp_tmp + 2];
			}
		}

		inv(b_y_tmp, dv);
		F[0] = dv[0];
		F[1] = dv[1];
		F[2] = dv[2];
		F[3] = dv[3];
		B[0] = FALL[2];
		B[1] = TALL[0] - ((PC[2] - (P1[2] * 0.5 + P2[2] * 0.5)) * Fp[0] + (PC[2] -
			(P3[2] * 0.5 + P4[2] * 0.5)) * Fp[1]);
		B[2] = TALL[1];
		for (i = 0; i < 2; i++) {
			PvL_idx_0 = 0.0;
			PvR_idx_0 = F[i + 2];
			for (y_tmp_tmp = 0; y_tmp_tmp < 3; y_tmp_tmp++) {
				i1 = y_tmp_tmp << 1;
				PvL_idx_0 += (F[i] * y_tmp[i1] + PvR_idx_0 * y_tmp[i1 + 1]) *
					B[y_tmp_tmp];
			}

			b_F[i] = PvL_idx_0;
		}

		FF_idx_2 = b_F[0];
		FH_idx_2 = b_F[1];

		/* ----横滚解算 */
		if (MODE[5] != '\x00') {
			/* 调节横滚时多余腿的位置 */
			if ((G[0] != '\x00') && (G[2] != '\x00')) {
				P4[0] = P1_reg_idx_0;
				P4[1] = P3_reg_idx_1;
				P4[2] = P1_reg_idx_2;
				P2[0] = P3_reg_idx_0;
				P2[1] = P1_reg_idx_1;
				P2[2] = P3_reg_idx_2;
			}
			else {
				P1[0] = P4_reg_idx_0;
				P1[1] = P2_reg_idx_1;
				P1[2] = P4_reg_idx_2;
				P3[0] = P2_reg_idx_0;
				P3[1] = P4_reg_idx_1;
				P3[2] = P2_reg_idx_2;
			}
		}

		/* 计算虚拟腿 */
		/* MX   Ffz */
		/* MY   Fhz */
		A[0] = 1.0;
		A[3] = 1.0;
		A[1] = -(PC[1] - (P1[1] * 0.5 + P4[1] * 0.5));
		A[4] = -(PC[1] - (P2[1] * 0.5 + P3[1] * 0.5));
		A[2] = PC[0] - (P1[0] * 0.5 + P4[0] * 0.5);
		A[5] = PC[0] - (P2[0] * 0.5 + P3[0] * 0.5);
		for (i = 0; i < 3; i++) {
			y_tmp_tmp = i << 1;
			y_tmp[y_tmp_tmp] = A[i];
			y_tmp[y_tmp_tmp + 1] = A[i + 3];
		}

		for (i = 0; i < 2; i++) {
			PvL_idx_0 = y_tmp[i + 2];
			PvR_idx_0 = y_tmp[i + 4];
			for (y_tmp_tmp = 0; y_tmp_tmp < 2; y_tmp_tmp++) {
				b_y_tmp[i + (y_tmp_tmp << 1)] = (y_tmp[i] * A[3 * y_tmp_tmp] + PvL_idx_0
					* A[3 * y_tmp_tmp + 1]) + PvR_idx_0 * A[3 * y_tmp_tmp + 2];
			}
		}

		inv(b_y_tmp, dv);
		F[0] = dv[0];
		F[1] = dv[1];
		F[2] = dv[2];
		F[3] = dv[3];
		B[0] = FALL[2];
		B[1] = TALL[0] - ((PC[2] - (P1[2] * 0.5 + P4[2] * 0.5)) * Fp[0] + (PC[2] -
			(P2[2] * 0.5 + P3[2] * 0.5)) * Fp[1]);
		B[2] = TALL[1];
		for (i = 0; i < 2; i++) {
			PvL_idx_0 = 0.0;
			PvR_idx_0 = F[i + 2];
			for (y_tmp_tmp = 0; y_tmp_tmp < 3; y_tmp_tmp++) {
				i1 = y_tmp_tmp << 1;
				PvL_idx_0 += (F[i] * y_tmp[i1] + PvR_idx_0 * y_tmp[i1 + 1]) *
					B[y_tmp_tmp];
			}

			b_F[i] = PvL_idx_0;
		}

		F1c[1] = TALL[2] / fabs(P1[0]) / 4.0;
		F2c[1] = TALL[2] / fabs(P2[0]) / 4.0;
		F3c[1] = TALL[2] / fabs(P3[0]) / 4.0;
		F4c[1] = TALL[2] / fabs(P4[0]) / 4.0;
		B_idx_0 = 0.0 * (1.0 - Div_Roll);
		B_idx_1 = 0.0 * Div_Roll;
		F1c[0] = B_idx_1 + B_idx_0;
		PvR_idx_0 = 0.0 * Div_Roll;
		F2c[0] = PvR_idx_0 + B_idx_0;
		F3c[0] = PvR_idx_0 + B_idx_0;
		F4c[0] = B_idx_1 + B_idx_0;
		B_idx_1 = Fp[0] / 2.0 * Div_Roll;
		PvL_idx_0 = Fp[0] / 2.0 * (1.0 - Div_Roll);
		F1c[1] = (B_idx_1 + PvL_idx_0) - F1c[1];
		PvR_idx_0 = Fp[1] / 2.0 * Div_Roll;
		F2c[1] += PvR_idx_0 + PvL_idx_0;
		B_idx_2 = Fp[1] / 2.0 * (1.0 - Div_Roll);
		F3c[1] += PvR_idx_0 + B_idx_2;
		F4c[1] = (B_idx_1 + B_idx_2) - F4c[1];
		B_idx_1 = FF_idx_2 / 2.0 * (1.0 - Div_Roll);
		PvL_idx_0 = b_F[0] / 2.0 * Div_Roll;
		F1c[2] = PvL_idx_0 + B_idx_1;
		PvR_idx_0 = b_F[1] / 2.0 * Div_Roll;
		F2c[2] = PvR_idx_0 + B_idx_1;
		B_idx_2 = FH_idx_2 / 2.0 * (1.0 - Div_Roll);
		F3c[2] = PvR_idx_0 + B_idx_2;
		F4c[2] = PvL_idx_0 + B_idx_2;
	}

	/*      逆+（从箭头往里看） */
	/*       z   y 逆+ */
	/*       |  / */
	/*       | / */
	/*       |/ */
	/*       o—————x 逆+ */
	/* 1FL   2FR */
	/* 4BL   3BR */
	/*      逆+（从箭头往里看） */
	/*       z3  y 逆+2 */
	/*       |  / */
	/*       | / */
	/*       |/ */
	/*       o—————x 逆+1 */
	/* 1FL   2FR* */
	/* 4BL   3BR  */
	if ((G_NUM == 4) && ((unsigned char)CAL_MODE[3] == 2)) {
		/* 全维度求解 */
		PvL_idx_0 = PC[0] - P2[0];
		PvR_idx_0 = PC[0] - P4[0];

		/* Fy */
		/* Fz */
		/* Mx 俯仰 */
		/* My 横滚 */
		/* Mz 转向 */
		b_B[0] = FALL[1] / 2.0;
		b_B[1] = FALL[2] / 2.0;
		b_B[2] = TALL[0] / 2.0;
		b_B[3] = TALL[1] / 2.0;
		b_B[4] = TALL[2] / 2.0;

		/* Mz 航向  */
		b_A[0] = 1.0;
		b_A[1] = 0.0;
		b_A[5] = 0.0;
		b_A[6] = 1.0;
		b_A[10] = 1.0;
		b_A[11] = 0.0;
		b_A[15] = 0.0;
		b_A[16] = 1.0;
		b_A[2] = PC[2] - P2[2];
		b_A[7] = -(PC[1] - P2[1]);
		b_A[12] = PC[2] - P4[2];
		b_A[17] = -(PC[1] - P4[1]);
		b_A[3] = 0.0;
		b_A[8] = PvL_idx_0;
		b_A[13] = 0.0;
		b_A[18] = PvR_idx_0;
		b_A[4] = -PvL_idx_0;
		b_A[9] = 0.0;
		b_A[14] = -PvR_idx_0;
		b_A[19] = 0.0;
		for (i = 0; i < 5; i++) {
			y_tmp_tmp = i << 2;
			c_y_tmp[y_tmp_tmp] = b_A[i];
			c_y_tmp[y_tmp_tmp + 1] = b_A[i + 5];
			c_y_tmp[y_tmp_tmp + 2] = b_A[i + 10];
			c_y_tmp[y_tmp_tmp + 3] = b_A[i + 15];
		}

		for (i = 0; i < 4; i++) {
			for (y_tmp_tmp = 0; y_tmp_tmp < 4; y_tmp_tmp++) {
				PvL_idx_0 = 0.0;
				for (i1 = 0; i1 < 5; i1++) {
					PvL_idx_0 += c_y_tmp[i + (i1 << 2)] * b_A[i1 + 5 * y_tmp_tmp];
				}

				d_y_tmp[i + (y_tmp_tmp << 2)] = PvL_idx_0;
			}
		}

		b_inv(d_y_tmp, c_A);
		for (i = 0; i < 4; i++) {
			PvL_idx_0 = 0.0;
			PvR_idx_0 = c_A[i + 4];
			B_idx_0 = c_A[i + 8];
			B_idx_1 = c_A[i + 12];
			for (y_tmp_tmp = 0; y_tmp_tmp < 5; y_tmp_tmp++) {
				i1 = y_tmp_tmp << 2;
				PvL_idx_0 += (((c_A[i] * c_y_tmp[i1] + PvR_idx_0 * c_y_tmp[i1 + 1]) +
					B_idx_0 * c_y_tmp[i1 + 2]) + B_idx_1 * c_y_tmp[i1 + 3]) *
					b_B[y_tmp_tmp];
			}

			F[i] = PvL_idx_0;
		}

		F2c[0] = 0.0;
		F2c[1] = F[0];
		F2c[2] = F[1];
		F4c[0] = 0.0;
		F4c[1] = F[2];
		F4c[2] = F[3];
		PvL_idx_0 = PC[0] - P1[0];
		PvR_idx_0 = PC[0] - P3[0];

		/* Fy */
		/* Fz */
		/* Mx 俯仰 */
		/* My 横滚 */
		/* Mz 转向 */
		/* Mz 航向  */
		b_A[0] = 1.0;
		b_A[1] = 0.0;
		b_A[5] = 0.0;
		b_A[6] = 1.0;
		b_A[10] = 1.0;
		b_A[11] = 0.0;
		b_A[15] = 0.0;
		b_A[16] = 1.0;
		b_A[2] = PC[2] - P1[2];
		b_A[7] = -(PC[1] - P1[1]);
		b_A[12] = PC[2] - P3[2];
		b_A[17] = -(PC[1] - P3[1]);
		b_A[3] = 0.0;
		b_A[8] = PvL_idx_0;
		b_A[13] = 0.0;
		b_A[18] = PvR_idx_0;
		b_A[4] = -PvL_idx_0;
		b_A[9] = 0.0;
		b_A[14] = -PvR_idx_0;
		b_A[19] = 0.0;
		for (i = 0; i < 5; i++) {
			y_tmp_tmp = i << 2;
			c_y_tmp[y_tmp_tmp] = b_A[i];
			c_y_tmp[y_tmp_tmp + 1] = b_A[i + 5];
			c_y_tmp[y_tmp_tmp + 2] = b_A[i + 10];
			c_y_tmp[y_tmp_tmp + 3] = b_A[i + 15];
		}

		for (i = 0; i < 4; i++) {
			for (y_tmp_tmp = 0; y_tmp_tmp < 4; y_tmp_tmp++) {
				PvL_idx_0 = 0.0;
				for (i1 = 0; i1 < 5; i1++) {
					PvL_idx_0 += c_y_tmp[i + (i1 << 2)] * b_A[i1 + 5 * y_tmp_tmp];
				}

				d_y_tmp[i + (y_tmp_tmp << 2)] = PvL_idx_0;
			}
		}

		b_inv(d_y_tmp, c_A);
		for (i = 0; i < 4; i++) {
			PvL_idx_0 = 0.0;
			PvR_idx_0 = c_A[i + 4];
			B_idx_0 = c_A[i + 8];
			B_idx_1 = c_A[i + 12];
			for (y_tmp_tmp = 0; y_tmp_tmp < 5; y_tmp_tmp++) {
				i1 = y_tmp_tmp << 2;
				PvL_idx_0 += (((c_A[i] * c_y_tmp[i1] + PvR_idx_0 * c_y_tmp[i1 + 1]) +
					B_idx_0 * c_y_tmp[i1 + 2]) + B_idx_1 * c_y_tmp[i1 + 3]) *
					b_B[y_tmp_tmp];
			}

			F[i] = PvL_idx_0;
		}

		F1c[0] = 0.0;
		F1c[1] = F[0];
		F1c[2] = F[1];
		F3c[0] = 0.0;
		F3c[1] = F[2];
		F3c[2] = F[3];
	}

	/*      逆+（从箭头往里看） */
	/*       z3  y 逆+2 */
	/*       |  / */
	/*       | / */
	/*       |/ */
	/*       o—————x 逆+1 */
	/* 1FL   2FR* */
	/* 4BL   3BR  */
	if ((G_NUM == 4) && ((unsigned char)CAL_MODE[3] == 3)) {
		/* 设计Y方向力 Z方向最小二乘解   */
		/* Fz */
		/* Mx  俯仰 */
		/* My 横滚 */
		B_idx_0 = FALL[2] / 2.0;
		B_idx_1 = TALL[0] / 2.0;
		B_idx_2 = TALL[1] / 2.0;

		/* Mz 航向  */
		A[0] = 1.0;
		A[3] = 1.0;
		A[1] = -(PC[1] - P2[1]);
		A[4] = -(PC[1] - P4[1]);
		A[2] = PC[0] - P2[0];
		A[5] = PC[0] - P4[0];

		/* Mx  俯仰 */
		/* Mz 航向  */
		for (i = 0; i < 3; i++) {
			y_tmp_tmp = i << 1;
			y_tmp[y_tmp_tmp] = A[i];
			y_tmp[y_tmp_tmp + 1] = A[i + 3];
		}

		for (i = 0; i < 2; i++) {
			PvL_idx_0 = y_tmp[i + 2];
			PvR_idx_0 = y_tmp[i + 4];
			for (y_tmp_tmp = 0; y_tmp_tmp < 2; y_tmp_tmp++) {
				b_y_tmp[i + (y_tmp_tmp << 1)] = (y_tmp[i] * A[3 * y_tmp_tmp] + PvL_idx_0
					* A[3 * y_tmp_tmp + 1]) + PvR_idx_0 * A[3 * y_tmp_tmp + 2];
			}
		}

		inv(b_y_tmp, dv);
		F[0] = dv[0];
		F[1] = dv[1];
		F[2] = dv[2];
		F[3] = dv[3];
		B[0] = B_idx_0;
		B[1] = B_idx_1 - ((PC[2] - P2[2]) * Fp[0] + (PC[2] - P4[2]) * Fp[1]) / 2.0;
		B[2] = B_idx_2;
		for (i = 0; i < 2; i++) {
			PvL_idx_0 = 0.0;
			PvR_idx_0 = F[i + 2];
			for (y_tmp_tmp = 0; y_tmp_tmp < 3; y_tmp_tmp++) {
				i1 = y_tmp_tmp << 1;
				PvL_idx_0 += (F[i] * y_tmp[i1] + PvR_idx_0 * y_tmp[i1 + 1]) *
					B[y_tmp_tmp];
			}

			b_F[i] = PvL_idx_0;
		}

		F2c[0] = 0.0;
		F2c[1] = Fp[0] / 2.0;
		F2c[2] = b_F[0];
		F4c[0] = 0.0;
		F4c[1] = Fp[1] / 2.0;
		F4c[2] = b_F[1];

		/* Fz */
		/* Mx  俯仰 */
		/* My 横滚 */
		/* Mz 航向  */
		A[0] = 1.0;
		A[3] = 1.0;
		A[1] = -(PC[1] - P1[1]);
		A[4] = -(PC[1] - P3[1]);
		A[2] = PC[0] - P1[0];
		A[5] = PC[0] - P3[0];

		/* Mx  俯仰 */
		/* Mz 航向  */
		for (i = 0; i < 3; i++) {
			y_tmp_tmp = i << 1;
			y_tmp[y_tmp_tmp] = A[i];
			y_tmp[y_tmp_tmp + 1] = A[i + 3];
		}

		for (i = 0; i < 2; i++) {
			PvL_idx_0 = y_tmp[i + 2];
			PvR_idx_0 = y_tmp[i + 4];
			for (y_tmp_tmp = 0; y_tmp_tmp < 2; y_tmp_tmp++) {
				b_y_tmp[i + (y_tmp_tmp << 1)] = (y_tmp[i] * A[3 * y_tmp_tmp] + PvL_idx_0
					* A[3 * y_tmp_tmp + 1]) + PvR_idx_0 * A[3 * y_tmp_tmp + 2];
			}
		}

		inv(b_y_tmp, dv);
		F[0] = dv[0];
		F[1] = dv[1];
		F[2] = dv[2];
		F[3] = dv[3];
		B[0] = B_idx_0;
		B[1] = B_idx_1 - ((PC[2] - P1[2]) * Fp[0] + (PC[2] - P3[2]) * Fp[1]) / 2.0;
		B[2] = B_idx_2;
		for (i = 0; i < 2; i++) {
			PvL_idx_0 = 0.0;
			PvR_idx_0 = F[i + 2];
			for (y_tmp_tmp = 0; y_tmp_tmp < 3; y_tmp_tmp++) {
				i1 = y_tmp_tmp << 1;
				PvL_idx_0 += (F[i] * y_tmp[i1] + PvR_idx_0 * y_tmp[i1 + 1]) *
					B[y_tmp_tmp];
			}

			b_F[i] = PvL_idx_0;
		}

		F1c[0] = 0.0;
		F1c[1] = Fp[0] / 2.0;
		F1c[2] = b_F[0];
		F3c[0] = 0.0;
		F3c[1] = Fp[1] / 2.0;
		F3c[2] = b_F[1];
	}

	/* ------------------------------------2 Leg对角------------------------------ */
	/*      逆+（从箭头往里看） */
	/*       z3  y 逆+2 */
	/*       |  / */
	/*       | / */
	/*       |/ */
	/*       o—————x 逆+1 */
	/* 1FL   2FR* */
	/* 4BL   3BR  */
	if ((G_NUM == 2) && ((unsigned char)CAL_MODE[1] == 0)) {
		/* 权重混合 前向 高度均有力控 */
		/* ----俯仰解算 */
		if ((G[0] != '\x00') && (G[2] != '\x00')) {
			P2[0] = P3_reg_idx_0;
			P2[1] = P1_reg_idx_1;
			P2[2] = P1_reg_idx_2;
			P4[0] = P1_reg_idx_0;
			P4[1] = P3_reg_idx_1;
			P4[2] = P3_reg_idx_2;
		}
		else {
			P1[0] = P4_reg_idx_0;
			P1[1] = P2_reg_idx_1;
			P1[2] = P2_reg_idx_2;
			P3[0] = P2_reg_idx_0;
			P3[1] = P4_reg_idx_1;
			P3[2] = P4_reg_idx_2;
		}

		/* 计算虚拟腿 */
		/* MX   Ffz */
		/* MY   Fhz */
		A[0] = 1.0;
		A[3] = 1.0;
		A[1] = -(PC[1] - (P1[1] * 0.5 + P2[1] * 0.5));
		A[4] = -(PC[1] - (P3[1] * 0.5 + P4[1] * 0.5));
		A[2] = PC[0] - (P1[0] * 0.5 + P2[0] * 0.5);
		A[5] = PC[0] - (P3[0] * 0.5 + P4[0] * 0.5);
		for (i = 0; i < 3; i++) {
			y_tmp_tmp = i << 1;
			y_tmp[y_tmp_tmp] = A[i];
			y_tmp[y_tmp_tmp + 1] = A[i + 3];
		}

		for (i = 0; i < 2; i++) {
			PvL_idx_0 = y_tmp[i + 2];
			PvR_idx_0 = y_tmp[i + 4];
			for (y_tmp_tmp = 0; y_tmp_tmp < 2; y_tmp_tmp++) {
				b_y_tmp[i + (y_tmp_tmp << 1)] = (y_tmp[i] * A[3 * y_tmp_tmp] + PvL_idx_0
					* A[3 * y_tmp_tmp + 1]) + PvR_idx_0 * A[3 * y_tmp_tmp + 2];
			}
		}

		inv(b_y_tmp, dv);
		F[0] = dv[0];
		F[1] = dv[1];
		F[2] = dv[2];
		F[3] = dv[3];
		for (i = 0; i < 2; i++) {
			PvL_idx_0 = F[i + 2];
			for (y_tmp_tmp = 0; y_tmp_tmp < 3; y_tmp_tmp++) {
				i1 = y_tmp_tmp << 1;
				A[i + i1] = F[i] * y_tmp[i1] + PvL_idx_0 * y_tmp[i1 + 1];
			}
		}

		B[1] = TALL[0] - ((PC[2] - (P1[2] * 0.5 + P2[2] * 0.5)) * Fp[0] + (PC[2] -
			(P3[2] * 0.5 + P4[2] * 0.5)) * Fp[1]);
		for (i = 0; i < 2; i++) {
			b_F[i] = (A[i] * FALL[2] + A[i + 2] * B[1]) + A[i + 4] * TALL[1];
		}

		FF_idx_2 = b_F[0] * 2.0;
		FH_idx_2 = b_F[1] * 2.0;

		/* ----横滚解算 */
		if ((G[0] != '\x00') && (G[2] != '\x00')) {
			P4[0] = P1_reg_idx_0;
			P4[1] = P3_reg_idx_1;
			P4[2] = P1_reg_idx_2;
			P2[0] = P3_reg_idx_0;
			P2[1] = P1_reg_idx_1;
			P2[2] = P3_reg_idx_2;
		}
		else {
			P1[0] = P4_reg_idx_0;
			P1[1] = P2_reg_idx_1;
			P1[2] = P4_reg_idx_2;
			P3[0] = P2_reg_idx_0;
			P3[1] = P4_reg_idx_1;
			P3[2] = P2_reg_idx_2;
		}

		/* 计算虚拟腿 */
		/* MX   Ffz */
		/* MY   Fhz */
		A[0] = 1.0;
		A[3] = 1.0;
		A[1] = -(PC[1] - (P1[1] * 0.5 + P4[1] * 0.5));
		A[4] = -(PC[1] - (P2[1] * 0.5 + P3[1] * 0.5));
		A[2] = PC[0] - (P1[0] * 0.5 + P4[0] * 0.5);
		A[5] = PC[0] - (P2[0] * 0.5 + P3[0] * 0.5);
		for (i = 0; i < 3; i++) {
			y_tmp_tmp = i << 1;
			y_tmp[y_tmp_tmp] = A[i];
			y_tmp[y_tmp_tmp + 1] = A[i + 3];
		}

		for (i = 0; i < 2; i++) {
			PvL_idx_0 = y_tmp[i + 2];
			PvR_idx_0 = y_tmp[i + 4];
			for (y_tmp_tmp = 0; y_tmp_tmp < 2; y_tmp_tmp++) {
				b_y_tmp[i + (y_tmp_tmp << 1)] = (y_tmp[i] * A[3 * y_tmp_tmp] + PvL_idx_0
					* A[3 * y_tmp_tmp + 1]) + PvR_idx_0 * A[3 * y_tmp_tmp + 2];
			}
		}

		inv(b_y_tmp, dv);
		F[0] = dv[0];
		F[1] = dv[1];
		F[2] = dv[2];
		F[3] = dv[3];
		for (i = 0; i < 2; i++) {
			PvL_idx_0 = F[i + 2];
			for (y_tmp_tmp = 0; y_tmp_tmp < 3; y_tmp_tmp++) {
				i1 = y_tmp_tmp << 1;
				A[i + i1] = F[i] * y_tmp[i1] + PvL_idx_0 * y_tmp[i1 + 1];
			}
		}

		B[1] = TALL[0] - ((PC[2] - (P1[2] * 0.5 + P4[2] * 0.5)) * Fp[0] + (PC[2] -
			(P2[2] * 0.5 + P3[2] * 0.5)) * Fp[1]);
		for (i = 0; i < 2; i++) {
			b_F[i] = (A[i] * FALL[2] + A[i + 2] * B[1]) + A[i + 4] * TALL[1];
		}

		F1c[1] = TALL[2] / fabs(P1[0]) / 4.0;
		F2c[1] = TALL[2] / fabs(P2[0]) / 4.0;
		F3c[1] = TALL[2] / fabs(P3[0]) / 4.0;
		F4c[1] = TALL[2] / fabs(P4[0]) / 4.0;
		B_idx_1 = 0.0 * (1.0 - Div_Roll);
		PvL_idx_0 = 0.0 * Div_Roll;
		F1c[0] = PvL_idx_0 + B_idx_1;
		PvR_idx_0 = 0.0 * Div_Roll;
		F2c[0] = PvR_idx_0 + B_idx_1;
		B_idx_2 = 0.0 * (1.0 - Div_Roll);
		F3c[0] = PvR_idx_0 + B_idx_2;
		F4c[0] = PvL_idx_0 + B_idx_2;
		B_idx_1 = Fp[0] / 2.0 * (1.0 - Div_Roll);
		PvL_idx_0 = Fp[0] / 2.0 * Div_Roll;
		F1c[1] = (PvL_idx_0 + B_idx_1) - F1c[1];
		PvR_idx_0 = Fp[1] / 2.0 * Div_Roll;
		F2c[1] += PvR_idx_0 + B_idx_1;
		B_idx_2 = Fp[1] / 2.0 * (1.0 - Div_Roll);
		F3c[1] += PvR_idx_0 + B_idx_2;
		F4c[1] = (PvL_idx_0 + B_idx_2) - F4c[1];
		B_idx_1 = FF_idx_2 / 2.0 * (1.0 - Div_Roll);
		PvL_idx_0 = b_F[0] * 2.0 / 2.0 * Div_Roll;
		F1c[2] = PvL_idx_0 + B_idx_1;
		PvR_idx_0 = b_F[1] * 2.0 / 2.0 * Div_Roll;
		F2c[2] = PvR_idx_0 + B_idx_1;
		B_idx_2 = FH_idx_2 / 2.0 * (1.0 - Div_Roll);
		F3c[2] = PvR_idx_0 + B_idx_2;
		F4c[2] = PvL_idx_0 + B_idx_2;
		F1c[1] *= 2.0;
		F2c[1] *= 2.0;
		F3c[1] *= 2.0;
		F4c[1] *= 2.0;
	}

	/*      逆+（从箭头往里看） */
	/*       z3  y 逆+2 */
	/*       |  / */
	/*       | / */
	/*       |/ */
	/*       o—————x 逆+1 */
	/* 1FL   2FR* */
	/* 4BL   3BR  */
	if ((G_NUM == 2) && ((unsigned char)CAL_MODE[1] == 1)) {
		/* 忽略Y方向前向力  需要使用前向SLIP摆动实现速度控制 nouse */
		if ((G[1] != '\x00') && (G[3] != '\x00')) {
			PvL_idx_0 = P2[0];
			PvR_idx_0 = P4[0];
			FH_idx_2 = P2[1];
			FF_idx_2 = P4[1];
			B_idx_1 = P2[2];
			PvR_idx_2 = P4[2];
		}
		else {
			PvL_idx_0 = P1[0];
			PvR_idx_0 = P3[0];
			FH_idx_2 = P1[1];
			FF_idx_2 = P3[1];
			B_idx_1 = P1[2];
			PvR_idx_2 = P3[2];
		}

		PvL_idx_0 = PC[0] - PvL_idx_0;
		PvR_idx_0 = PC[0] - PvR_idx_0;
		FH_idx_2 = PC[1] - FH_idx_2;
		FF_idx_2 = PC[1] - FF_idx_2;
		B_idx_1 = PC[2] - B_idx_1;
		PvR_idx_2 = PC[2] - PvR_idx_2;

		/* Fz */
		/* Mx  俯仰 */
		/* My 横滚 */
		/* Mz 航向  */
		c_A[0] = 0.0;
		c_A[4] = 1.0;
		c_A[8] = 0.0;
		c_A[12] = 1.0;
		c_A[1] = B_idx_1;
		c_A[5] = -FH_idx_2;
		c_A[9] = PvR_idx_2;
		c_A[13] = -FF_idx_2;
		c_A[2] = 0.0;
		c_A[6] = PvL_idx_0;
		c_A[10] = 0.0;
		c_A[14] = PvR_idx_0;
		c_A[3] = -PvL_idx_0;
		c_A[7] = 0.0;
		c_A[11] = -PvR_idx_0;
		c_A[15] = 0.0;

		/*  -(PvF(3)*Fp(1)+PvH(3)*Fp(2));%Mx  俯仰 */
		/*  (-PvF(1)*Fp(1)-PvH(1)*Fp(2)); */
		/* Mz 航向  */
		for (i = 0; i < 4; i++) {
			y_tmp_tmp = i << 2;
			d_y_tmp[y_tmp_tmp] = c_A[i];
			d_y_tmp[y_tmp_tmp + 1] = c_A[i + 4];
			d_y_tmp[y_tmp_tmp + 2] = c_A[i + 8];
			d_y_tmp[y_tmp_tmp + 3] = c_A[i + 12];
		}

		for (i = 0; i < 4; i++) {
			PvL_idx_0 = d_y_tmp[i + 4];
			PvR_idx_0 = d_y_tmp[i + 8];
			B_idx_0 = d_y_tmp[i + 12];
			for (y_tmp_tmp = 0; y_tmp_tmp < 4; y_tmp_tmp++) {
				i1 = y_tmp_tmp << 2;
				e_y_tmp[i + i1] = ((d_y_tmp[i] * c_A[i1] + PvL_idx_0 * c_A[i1 + 1]) +
					PvR_idx_0 * c_A[i1 + 2]) + B_idx_0 * c_A[i1 + 3];
			}
		}

		b_inv(e_y_tmp, c_A);
		b_y_tmp[0] = FALL[2];
		b_y_tmp[1] = TALL[0];
		b_y_tmp[2] = TALL[1];
		b_y_tmp[3] = TALL[2];
		for (i = 0; i < 4; i++) {
			PvL_idx_0 = 0.0;
			PvR_idx_0 = c_A[i + 4];
			B_idx_0 = c_A[i + 8];
			B_idx_1 = c_A[i + 12];
			for (y_tmp_tmp = 0; y_tmp_tmp < 4; y_tmp_tmp++) {
				i1 = y_tmp_tmp << 2;
				PvL_idx_0 += (((c_A[i] * d_y_tmp[i1] + PvR_idx_0 * d_y_tmp[i1 + 1]) +
					B_idx_0 * d_y_tmp[i1 + 2]) + B_idx_1 * d_y_tmp[i1 + 3]) *
					b_y_tmp[y_tmp_tmp];
			}

			F[i] = PvL_idx_0;
		}

		if ((G[1] != '\x00') && (G[3] != '\x00')) {
			F2c[0] = 0.0;
			F4c[0] = 0.0;
			F2c[1] = F[0];
			F4c[1] = F[2];
			F2c[2] = F[1];
			F4c[2] = F[3];
		}
		else {
			F1c[0] = 0.0;
			F3c[0] = 0.0;
			F1c[1] = F[0];
			F3c[1] = F[2];
			F1c[2] = F[1];
			F3c[2] = F[3];
		}
	}

	/*      逆+（从箭头往里看） */
	/*       z3  y 逆+2 */
	/*       |  / */
	/*       | / */
	/*       |/ */
	/*       o—————x 逆+1 */
	/* 1FL   2FR* */
	/* 4BL   3BR  */
	if ((G_NUM == 2) && ((unsigned char)CAL_MODE[1] == 2)) {
		/* 全维度求解 */
		if ((G[1] != '\x00') && (G[3] != '\x00')) {
			PvL_idx_0 = P2[0];
			PvR_idx_0 = P4[0];
			FH_idx_2 = P2[1];
			FF_idx_2 = P4[1];
			B_idx_1 = P2[2];
			PvR_idx_2 = P4[2];
		}
		else {
			PvL_idx_0 = P1[0];
			PvR_idx_0 = P3[0];
			FH_idx_2 = P1[1];
			FF_idx_2 = P3[1];
			B_idx_1 = P1[2];
			PvR_idx_2 = P3[2];
		}

		PvL_idx_0 = PC[0] - PvL_idx_0;
		PvR_idx_0 = PC[0] - PvR_idx_0;
		FH_idx_2 = PC[1] - FH_idx_2;
		FF_idx_2 = PC[1] - FF_idx_2;
		B_idx_1 = PC[2] - B_idx_1;
		PvR_idx_2 = PC[2] - PvR_idx_2;

		/* Fy */
		/* Fz */
		/* Mx 俯仰 */
		/* My 横滚 */
		/* Mz 转向 */
		/* Mz 航向  */
		b_A[0] = 1.0;
		b_A[1] = 0.0;
		b_A[5] = 0.0;
		b_A[6] = 1.0;
		b_A[10] = 1.0;
		b_A[11] = 0.0;
		b_A[15] = 0.0;
		b_A[16] = 1.0;
		b_A[2] = B_idx_1;
		b_A[7] = -FH_idx_2;
		b_A[12] = PvR_idx_2;
		b_A[17] = -FF_idx_2;
		b_A[3] = 0.0;
		b_A[8] = PvL_idx_0;
		b_A[13] = 0.0;
		b_A[18] = PvR_idx_0;
		b_A[4] = -PvL_idx_0;
		b_A[9] = 0.0;
		b_A[14] = -PvR_idx_0;
		b_A[19] = 0.0;
		for (i = 0; i < 5; i++) {
			y_tmp_tmp = i << 2;
			c_y_tmp[y_tmp_tmp] = b_A[i];
			c_y_tmp[y_tmp_tmp + 1] = b_A[i + 5];
			c_y_tmp[y_tmp_tmp + 2] = b_A[i + 10];
			c_y_tmp[y_tmp_tmp + 3] = b_A[i + 15];
		}

		for (i = 0; i < 4; i++) {
			for (y_tmp_tmp = 0; y_tmp_tmp < 4; y_tmp_tmp++) {
				PvL_idx_0 = 0.0;
				for (i1 = 0; i1 < 5; i1++) {
					PvL_idx_0 += c_y_tmp[i + (i1 << 2)] * b_A[i1 + 5 * y_tmp_tmp];
				}

				d_y_tmp[i + (y_tmp_tmp << 2)] = PvL_idx_0;
			}
		}

		b_inv(d_y_tmp, c_A);
		b_B[0] = FALL[1];
		b_B[1] = FALL[2];
		b_B[2] = TALL[0];
		b_B[3] = TALL[1];
		b_B[4] = TALL[2];
		for (i = 0; i < 4; i++) {
			PvL_idx_0 = 0.0;
			PvR_idx_0 = c_A[i + 4];
			B_idx_0 = c_A[i + 8];
			B_idx_1 = c_A[i + 12];
			for (y_tmp_tmp = 0; y_tmp_tmp < 5; y_tmp_tmp++) {
				i1 = y_tmp_tmp << 2;
				PvL_idx_0 += (((c_A[i] * c_y_tmp[i1] + PvR_idx_0 * c_y_tmp[i1 + 1]) +
					B_idx_0 * c_y_tmp[i1 + 2]) + B_idx_1 * c_y_tmp[i1 + 3]) *
					b_B[y_tmp_tmp];
			}

			F[i] = PvL_idx_0;
		}

		if ((G[1] != '\x00') && (G[3] != '\x00')) {
			F2c[0] = 0.0;
			F4c[0] = 0.0;
			F2c[1] = F[0];
			F4c[1] = F[2];
			F2c[2] = F[1];
			F4c[2] = F[3];
		}
		else {
			F1c[0] = 0.0;
			F3c[0] = 0.0;
			F1c[1] = F[0];
			F3c[1] = F[2];
			F1c[2] = F[1];
			F3c[2] = F[3];
		}
	}

	/*      逆+（从箭头往里看） */
	/*       z3  y 逆+2 */
	/*       |  / */
	/*       | / */
	/*       |/ */
	/*       o—————x 逆+1 */
	/* 1FL   2FR* */
	/* 4BL   3BR  */
	if ((G_NUM == 2) && ((unsigned char)CAL_MODE[1] == 3)) {
		/* 设计Y方向力 Z方向最小二乘解 */
		if ((G[1] != '\x00') && (G[3] != '\x00')) {
			PvL_idx_0 = P2[0];
			PvR_idx_0 = P4[0];
			FH_idx_2 = P2[1];
			FF_idx_2 = P4[1];
			B_idx_1 = P2[2];
			PvR_idx_2 = P4[2];
		}
		else {
			PvL_idx_0 = P1[0];
			PvR_idx_0 = P3[0];
			FH_idx_2 = P1[1];
			FF_idx_2 = P3[1];
			B_idx_1 = P1[2];
			PvR_idx_2 = P3[2];
		}

		PvL_idx_0 = PC[0] - PvL_idx_0;
		PvR_idx_0 = PC[0] - PvR_idx_0;
		FH_idx_2 = PC[1] - FH_idx_2;
		FF_idx_2 = PC[1] - FF_idx_2;
		B_idx_1 = PC[2] - B_idx_1;
		PvR_idx_2 = PC[2] - PvR_idx_2;

		/* Fz */
		/* Mx  俯仰 */
		/* My 横滚 */
		/* Mz 航向  */
		A[0] = 1.0;
		A[3] = 1.0;
		A[1] = -FH_idx_2;
		A[4] = -FF_idx_2;
		A[2] = PvL_idx_0;
		A[5] = PvR_idx_0;

		/* Mx  俯仰 */
		/* Mz 航向  */
		for (i = 0; i < 3; i++) {
			y_tmp_tmp = i << 1;
			y_tmp[y_tmp_tmp] = A[i];
			y_tmp[y_tmp_tmp + 1] = A[i + 3];
		}

		for (i = 0; i < 2; i++) {
			PvL_idx_0 = y_tmp[i + 2];
			PvR_idx_0 = y_tmp[i + 4];
			for (y_tmp_tmp = 0; y_tmp_tmp < 2; y_tmp_tmp++) {
				b_y_tmp[i + (y_tmp_tmp << 1)] = (y_tmp[i] * A[3 * y_tmp_tmp] + PvL_idx_0
					* A[3 * y_tmp_tmp + 1]) + PvR_idx_0 * A[3 * y_tmp_tmp + 2];
			}
		}

		inv(b_y_tmp, dv);
		F[0] = dv[0];
		F[1] = dv[1];
		F[2] = dv[2];
		F[3] = dv[3];
		for (i = 0; i < 2; i++) {
			PvL_idx_0 = F[i + 2];
			for (y_tmp_tmp = 0; y_tmp_tmp < 3; y_tmp_tmp++) {
				i1 = y_tmp_tmp << 1;
				A[i + i1] = F[i] * y_tmp[i1] + PvL_idx_0 * y_tmp[i1 + 1];
			}
		}

		B[1] = TALL[0] - (B_idx_1 * Fp[0] + PvR_idx_2 * Fp[1]);
		B[2] = TALL[1];
		for (i = 0; i < 2; i++) {
			b_F[i] = (A[i] * FALL[2] + A[i + 2] * B[1]) + A[i + 4] * B[2];
		}

		if ((G[1] != '\x00') && (G[3] != '\x00')) {
			F2c[0] = 0.0;
			F4c[0] = 0.0;
			F2c[1] = Fp[0];
			F4c[1] = Fp[1];
			F2c[2] = b_F[0];
			F4c[2] = b_F[1];
		}
		else {
			F1c[0] = 0.0;
			F3c[0] = 0.0;
			F1c[1] = Fp[0];
			F3c[1] = Fp[1];
			F1c[2] = b_F[0];
			F3c[2] = b_F[1];
		}
	}

	/* ------------------------------------保护 */
	if (fabs(ATT_NOW[1]) > 0.0) {
		/* 1FL   2FR */
		/* 4BL   3BR */
		if (MODE[6] != '\x00') {
			PvL_idx_0 = ATT_NOW[1];
			b_cosd(&PvL_idx_0);
			F4c[2] /= PvL_idx_0;
			F1c[2] /= PvL_idx_0;

			/* 转换固定腿部坐标系 横滚角补偿 */
			F2c[2] /= PvL_idx_0;

			/* 转换固定腿部坐标系 横滚角补偿 */
			F3c[2] /= PvL_idx_0;
		}
		else {
			PvL_idx_0 = ATT_NOW[1];
			b_sind(&PvL_idx_0);
			F4c[2] *= PvL_idx_0 + 1.0;
			PvR_idx_0 = ATT_NOW[1];
			b_sind(&PvR_idx_0);
			F1c[2] *= PvR_idx_0 + 1.0;

			/* 转换固定腿部坐标系 横滚角补偿 */
			F2c[2] *= 1.0 - PvL_idx_0;

			/* 转换固定腿部坐标系 横滚角补偿 */
			F3c[2] *= 1.0 - PvL_idx_0;
		}
	}

	/* --------------控制反向修正输出-------------- */
	B_idx_1 = PC[2] - P1_reg_idx_2;
	PvR_idx_2 = PC[2] - P2_reg_idx_2;
	B_idx_2 = PC[2] - P3_reg_idx_2;
	FF_idx_2 = PC[2] - P4_reg_idx_2;
	B_idx_0 = (((F1c[1] * B_idx_1 - F1c[2] * (PC[1] - P1_reg_idx_1)) * (double)
		G_NUM_tmp + (F2c[1] * PvR_idx_2 - F2c[2] * (PC[1] - P2_reg_idx_1))
		* (double)b_G_NUM_tmp) + (F3c[1] * B_idx_2 - F3c[2] * (PC[1] -
			P3_reg_idx_1)) * (double)c_G_NUM_tmp) + (F4c[1] * FF_idx_2 - F4c[2] * (PC[1]
				- P4_reg_idx_1)) * (double)d_G_NUM_tmp;
	FH_idx_2 = (((F1c[2] * (PC[0] - P1_reg_idx_0) - F1c[0] * B_idx_1) * (double)
		G_NUM_tmp + (F2c[2] * (PC[0] - P2_reg_idx_0) - F2c[0] * PvR_idx_2)
		* (double)b_G_NUM_tmp) + (F3c[2] * (PC[0] - P3_reg_idx_0) - F3c[0]
			* B_idx_2) * (double)c_G_NUM_tmp) + (F4c[2] * (PC[0] - P4_reg_idx_0) - F4c[0]
				* FF_idx_2) * (double)d_G_NUM_tmp;

	/* x y z */
	if (TALL[0] * B_idx_0 < 0.0) {
		/* 俯仰反向 */
		FF_idx_2 = fabs(FH_idx_2) / P2_reg_idx_0 / Div_Roll;
		B_idx_2 = -B_idx_0 / P1_reg_idx_1;
		PvR_idx_0 = B_idx_2;
		if (B_idx_2 > FF_idx_2) {
			PvR_idx_0 = FF_idx_2;
		}

		if (B_idx_2 < -FF_idx_2) {
			PvR_idx_0 = -FF_idx_2;
		}

		B_idx_2 = -B_idx_0 / P2_reg_idx_1;
		PvL_idx_0 = B_idx_2;
		if (B_idx_2 > FF_idx_2) {
			PvL_idx_0 = FF_idx_2;
		}

		if (B_idx_2 < -FF_idx_2) {
			PvL_idx_0 = -FF_idx_2;
		}

		B_idx_2 = -B_idx_0 / P3_reg_idx_1;
		B_idx_1 = B_idx_2;
		if (B_idx_2 > FF_idx_2) {
			B_idx_1 = FF_idx_2;
		}

		if (B_idx_2 < -FF_idx_2) {
			B_idx_1 = -FF_idx_2;
		}

		B_idx_2 = -B_idx_0 / P4_reg_idx_1;
		B_idx_0 = B_idx_2;
		if (B_idx_2 > FF_idx_2) {
			B_idx_0 = FF_idx_2;
		}

		if (B_idx_2 < -FF_idx_2) {
			B_idx_0 = -FF_idx_2;
		}

		if (MODE[0] != '\x00') {
			/* disp('俯仰反向'); */
			F1c[2] += PvR_idx_0;
			F2c[2] += PvL_idx_0;
			F3c[2] += B_idx_1;
			F4c[2] += B_idx_0;
		}
	}

	if ((TALL[1] * FH_idx_2 < 0.0) && (MODE[1] != '\x00')) {
		/*  */
		/* disp('横滚反向'); */
		F1c[2] += -FH_idx_2 / P1_reg_idx_0;
		F2c[2] += -FH_idx_2 / P2_reg_idx_0;
		F3c[2] += -FH_idx_2 / P3_reg_idx_0;
		F4c[2] += -FH_idx_2 / P4_reg_idx_0;
	}

	/* --------------保护输出-------------- */
	B_idx_0 = 0.0;
	if ((G[0] != '\x00') && (0.0 > F1c[2])) {
		B_idx_0 = F1c[2];
	}

	if ((G[1] != '\x00') && (B_idx_0 > F2c[2])) {
		B_idx_0 = F2c[2];
	}

	if ((G[2] != '\x00') && (B_idx_0 > F3c[2])) {
		B_idx_0 = F3c[2];
	}

	if ((G[3] != '\x00') && (B_idx_0 > F4c[2])) {
		B_idx_0 = F4c[2];
	}

	if ((B_idx_0 < 0.0) && (MODE[2] != '\x00')) {
		/* 负数修正 */
		if (G[0] != '\x00') {
			F1c[2] -= B_idx_0;
		}

		if (G[1] != '\x00') {
			F2c[2] -= B_idx_0;
		}

		if (G[2] != '\x00') {
			F3c[2] -= B_idx_0;
		}

		if (G[3] != '\x00') {
			F4c[2] -= B_idx_0;
		}
	}

	if ((G_NUM > 0) && (F1c[2] >= 0.0) && (F2c[2] >= 0.0) && (F3c[2] >= 0.0) &&
		(F4c[2] >= 0.0) && (MODE[3] != '\x00')) {
		/* 尺寸修正  */
		B_idx_0 = FALL[2] / (((F1c[2] * (double)G_NUM_tmp + F2c[2] * (double)
			b_G_NUM_tmp) + F3c[2] * (double)c_G_NUM_tmp) + F4c[2] * (double)
			d_G_NUM_tmp);
		if (G[0] != '\x00') {
			F1c[2] *= B_idx_0;
		}

		if (G[1] != '\x00') {
			F2c[2] *= B_idx_0;
		}

		if (G[2] != '\x00') {
			F3c[2] *= B_idx_0;
		}

		if (G[3] != '\x00') {
			F4c[2] *= B_idx_0;
		}
	}

	/* 限幅度 */
	if (G[0] != '\x00') {
		B_idx_2 = F1c[2];
		if (F1c[2] > 99.0) {
			F1c[2] = 99.0;
		}

		if (B_idx_2 < 0.0) {
			F1c[2] = 0.0;
		}
	}

	if (G[1] != '\x00') {
		B_idx_2 = F2c[2];
		if (F2c[2] > 99.0) {
			F2c[2] = 99.0;
		}

		if (B_idx_2 < 0.0) {
			F2c[2] = 0.0;
		}
	}

	if (G[2] != '\x00') {
		B_idx_2 = F3c[2];
		if (F3c[2] > 99.0) {
			F3c[2] = 99.0;
		}

		if (B_idx_2 < 0.0) {
			F3c[2] = 0.0;
		}
	}

	if (G[3] != '\x00') {
		B_idx_2 = F4c[2];
		if (F4c[2] > 99.0) {
			F4c[2] = 99.0;
		}

		if (B_idx_2 < 0.0) {
			F4c[2] = 0.0;
		}
	}
}

/*
 * Arguments    : void
 * Return Type  : void
 */
void force_dis_8new_initialize(void)
{
	//rt_InitInfAndNaN();
	isInitialized_force_dis_8new = true;
}

/*
 * Arguments    : void
 * Return Type  : void
 */
void force_dis_8new_terminate(void)
{
	/* (no terminate code required) */
	isInitialized_force_dis_8new = false;
}

/*
 * File trailer for force_dis_8new.c
 *
 * [EOF]
 */
